/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : tran2PC.cpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : Nov 12, 2020
  ******************************************************************************
  */
/* USER CODE END Header */
#include "tran2PC.hpp"
#include "string.h"

TRAN2PC tran2PC(USART3,(char *)"tran2PC");

void USART3_IRQHandler(void)
{
	if(LL_USART_IsActiveFlag_IDLE(USART3))
	{
		LL_USART_ClearFlag_IDLE(USART3);
		LL_DMA_DisableStream(DMA1,LL_DMA_STREAM_1);
		LL_DMA_ClearFlag_DME1(DMA1);
		LL_DMA_ClearFlag_HT1(DMA1);
		LL_DMA_ClearFlag_TC1(DMA1);
		LL_DMA_ClearFlag_TE1(DMA1);
		LL_DMA_ClearFlag_FE1(DMA1);
		tran2PC.RxDataSize = TRAN2PC_RX_LEN - LL_DMA_GetDataLength(DMA1, LL_DMA_STREAM_1);

		do{


			if(tran2PC.LockRx == HAL_LOCKED)break;
			tran2PC.LockRx = HAL_LOCKED;
			memcpy(tran2PC.RxDat,tran2PC.RxRawDat,tran2PC.RxDataSize);
			tran2PC.RxDat[tran2PC.RxDataSize]=0;
			tran2PC.LockRx = HAL_UNLOCKED;
			tran2PC.RxFlag = true;   //收到完整一帧


			BaseType_t YieldRequired = xTaskResumeFromISR(linuxTaskHandle);
			if(YieldRequired==pdTRUE)
			{
				/*如果函数xTaskResumeFromISR()返回值为pdTRUE，那么说明要恢复的这个
				任务的任务优先级等于或者高于正在运行的任务(被中断打断的任务),所以在
				退出中断的时候一定要进行上下文切换！*/
				portYIELD_FROM_ISR(YieldRequired);
			}
		}while(0);

		LL_DMA_SetDataLength(DMA1, LL_DMA_STREAM_1, TRAN2PC_RX_LEN);
		LL_DMA_EnableStream(DMA1, LL_DMA_STREAM_1);
	}
}

void TRAN2PC::tran2PC_Init(void)
{
	LockRx = HAL_UNLOCKED;
	RxFlag = false;
	RxDataSize = 0;
	executionTime_us = 0;
	Sta = STA_INI;
	Err = ERR_NONE;

	linux_init_success = false;
	linux.linux_init_success = false;
	xQueueOverwrite(queueLinux,&linux);
	/* 配置接收DMA */
	LL_DMA_DisableStream(DMA1,LL_DMA_STREAM_1);
	LL_DMA_SetPeriphAddress(DMA1, LL_DMA_STREAM_1, (uint32_t)&huart->RDR);
	LL_DMA_SetMemoryAddress(DMA1, LL_DMA_STREAM_1, (uint32_t)RxRawDat);
	LL_DMA_SetDataLength(DMA1, LL_DMA_STREAM_1, TRAN2PC_RX_LEN);
	LL_DMA_EnableStream(DMA1, LL_DMA_STREAM_1);
	/* 配置接收DMA */

	/* 配置发送DMA */
	LL_DMA_DisableStream(DMA1,LL_DMA_STREAM_3);
	LL_DMA_SetPeriphAddress(DMA1, LL_DMA_STREAM_3, (uint32_t)&huart->TDR);
	LL_DMA_SetMemoryAddress(DMA1, LL_DMA_STREAM_3, (uint32_t)TxDat);
	LL_DMA_SetDataLength(DMA1, LL_DMA_STREAM_3, TRAN2PC_TX_LEN);
	LL_DMA_ClearFlag_TC3(DMA1);
	LL_DMA_EnableIT_TC(DMA1, LL_DMA_STREAM_3);
	LL_DMA_EnableStream(DMA1, LL_DMA_STREAM_3);
	/* 配置发送DMA */

	LL_USART_EnableDMAReq_RX(huart);
	LL_USART_EnableDMAReq_TX(huart);
	LL_USART_ClearFlag_IDLE(huart);
	LL_USART_EnableIT_IDLE(huart);
	Sta = STA_RUN;

	slam.sequence = 0;
	slam.slam_pose[0] = 0;
	slam.slam_pose[1] = 0;
	slam.slam_pose[2] = 0;
	slam.slam_valid_flag = 0;
	xQueueOverwrite(queueSLAM,&slam);
	osDelay(100);
}



void DMA1_Stream1_IRQHandler(void)  //接收DMA中断
{

}


void DMA1_Stream3_IRQHandler(void)  //发送DMA中断
{
	LL_DMA_DisableStream(DMA1,LL_DMA_STREAM_3);
	LL_DMA_ClearFlag_TC3(DMA1);
	tran2PC.TxFlag = false;
}


bool TRAN2PC::uart_Send_DMA(uint8_t * pData,uint16_t Size)
{
	if(TxFlag == true) return false;	//串口发送忙,放弃发送该帧数据
	LL_DMA_DisableStream(DMA1,LL_DMA_STREAM_3);

	LL_DMA_SetDataLength(DMA1, LL_DMA_STREAM_3, Size);
	LL_DMA_SetMemoryAddress(DMA1, LL_DMA_STREAM_3, (uint32_t)pData);
	LL_DMA_EnableStream(DMA1, LL_DMA_STREAM_3);
	TxFlag = true;
	return true;
}


bool TRAN2PC::uart_Send_PCmsg(){
	if(TxFlag == true) return false;
//	static u16 index = 0;
	xQueuePeek(queueCamTrigger,&camTrigger, 0);
	xQueuePeek(queueAccRaw,&accraw, 0);
	xQueuePeek(queueGyroRaw,&gyroraw, 0);
	xQueuePeek(queueQuaternions,&qua, 0);
	TxDat[0] = 0xfd;
	TxDat[1] = 0xfe;

	u8 temp1;
	u16 temp2;
	float tempf;


	temp2 = gyroraw.imu_timestamp/1000;
	TxDat[2] = BYTE0(temp2);
	TxDat[3] = BYTE1(temp2);

	//cam
	temp1 = camTrigger.pic_index;
	TxDat[4] = temp1;

	temp2= camTrigger.pic_timestamp/1000;
	TxDat[5] = BYTE0(temp2);
	TxDat[6] = BYTE1(temp2);
	//gyro
	temp2= gyroraw.gyro[0];
	TxDat[7] = BYTE0(temp2);
	TxDat[8] = BYTE1(temp2);

	temp2= gyroraw.gyro[1];
	TxDat[9] = BYTE0(temp2);
	TxDat[10] = BYTE1(temp2);

	temp2= gyroraw.gyro[2];
	TxDat[11] = BYTE0(temp2);
	TxDat[12] = BYTE1(temp2);

	//acc
	temp2= accraw.acc[0];
	TxDat[13] = BYTE0(temp2);
	TxDat[14] = BYTE1(temp2);

	temp2= accraw.acc[1];
	TxDat[15] = BYTE0(temp2);
	TxDat[16] = BYTE1(temp2);

	temp2= accraw.acc[2];
	TxDat[17] = BYTE0(temp2);
	TxDat[18] = BYTE1(temp2);
	//q
	tempf = qua.q[0];
	TxDat[19] = BYTE0(tempf);
	TxDat[20] = BYTE1(tempf);
	TxDat[21] = BYTE2(tempf);
	TxDat[22] = BYTE3(tempf);

	tempf = qua.q[1];
	TxDat[23] = BYTE0(tempf);
	TxDat[24] = BYTE1(tempf);
	TxDat[25] = BYTE2(tempf);
	TxDat[26] = BYTE3(tempf);

	tempf = qua.q[2];
	TxDat[27] = BYTE0(tempf);
	TxDat[28] = BYTE1(tempf);
	TxDat[29] = BYTE2(tempf);
	TxDat[30] = BYTE3(tempf);

	tempf = qua.q[3];
	TxDat[31] = BYTE0(tempf);
	TxDat[32] = BYTE1(tempf);
	TxDat[33] = BYTE2(tempf);
	TxDat[34] = BYTE3(tempf);

	uint8_t sum = 0;
	for(uint8_t i=0; i<35; i++) sum += TxDat[i];
	TxDat[35] = sum;


	TxDat[36] = 0xf2;
	TxDat[37] = 0xf1;

	return uart_Send_DMA((u8 *)TxDat, 38);

}



extern "C" void linux_main(void *argument)
{
	for(;;)
	{
		vTaskSuspend(linuxTaskHandle);
		if(tran2PC.RxDataSize==3){
			if(tran2PC.RxDat[0]==0xfe
					&&tran2PC.RxDat[1]==0xfd
					&&tran2PC.RxDat[2]==0xfc){
				tran2PC.linux_init_success = true;
				tran2PC.linux.linux_init_success = true;
				xQueueOverwrite(queueLinux,&tran2PC.linux);
			}
			if(tran2PC.RxDat[0]==0xfc
					&&tran2PC.RxDat[1]==0xfd
					&&tran2PC.RxDat[2]==0xfe){
				tran2PC.linux_init_success = false;
				tran2PC.linux.linux_init_success = false;
				xQueueOverwrite(queueLinux,&tran2PC.linux);
			}
		}
		else if(tran2PC.RxDataSize==18){
			BIT16 tmp1;
			BIT32 tmp2;
			if(tran2PC.RxDat[0]==0xf1&&tran2PC.RxDat[1]==0xf2
					&&tran2PC.RxDat[15]==0xf2&&tran2PC.RxDat[16]==0xf1){
				uint8_t sum = 0;
				for(int i=0;i<17;i++) sum += tran2PC.RxDat[i];
				if(sum==tran2PC.RxDat[17]){
//					tmp1.uc[0] = tran2PC.RxDat[2];
//					tmp1.uc[1] = tran2PC.RxDat[3];
//					tran2PC.slam.sequence = tmp1.ss;

					tmp2.uc[0] = tran2PC.RxDat[2];
					tmp2.uc[1] = tran2PC.RxDat[3];
					tmp2.uc[2] = tran2PC.RxDat[4];
					tmp2.uc[3] = tran2PC.RxDat[5];
					tran2PC.slam.slam_pose[0] = tmp2.fp*100.f;

					tmp2.uc[0] = tran2PC.RxDat[6];
					tmp2.uc[1] = tran2PC.RxDat[7];
					tmp2.uc[2] = tran2PC.RxDat[8];
					tmp2.uc[3] = tran2PC.RxDat[9];
					tran2PC.slam.slam_pose[1] = tmp2.fp*100.f;

					tmp2.uc[0] = tran2PC.RxDat[10];
					tmp2.uc[1] = tran2PC.RxDat[11];
					tmp2.uc[2] = tran2PC.RxDat[12];
					tmp2.uc[3] = tran2PC.RxDat[13];
					tran2PC.slam.slam_pose[2] = tmp2.fp*100.f;

					tran2PC.slam.slam_valid_flag = tran2PC.RxDat[14];
					tran2PC.slam.sequence++;
					xQueueOverwrite(queueSLAM,&tran2PC.slam);
				}
			}

		}

	}
}

extern "C" void tran2PC_main(void *argument)
{
	tran2PC.tran2PC_Init();
	osDelay(10);
	for(;;)
	{
		osSemaphoreAcquire(semTran2PC,0xffffffff);
		if(tran2PC.linux_init_success){
//			tran2PC.uart_Send_PCmsg();
			led3_Tog;
			led4_Tog;
		}
	}
}


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
